/**
*Copyright 2012 Rocky Mountain High School Technical Student Association
*
* This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*
*/
int MASTERS[10]; //for just slaving motors together
int SLAVES[10];  //listed in order added and holds port num
float PID_KP=30;  //proportional gain
float PID_KI=45; //integral gain
float PID_KD=50;  //derivative gain
float PID_Slew_UP=30; //slew upwards
float PID_Slew_Down=30; //slew it down
int SAMPLE_TIME=50;     //the time to wait between samples
int MONITORING[10];   //the motors we are monitoring listed in order and holding port num
int CONTROLLING[10]; //motors being controlled ''
int POWER[10];  //the powers of the motors being controlled listed by port number
int LAST_ENCODE[10];  //the last encoder value
float ERROR_P[10];    //the instant error p
float ERROR_I[10];   //the integral error I
float ERROR_D[10];  //the derivative of the error
int MOTOR_TARGET[10]; //array of motors that the motor references for it's target index is port num and value is port num of the
float MOTOR_SPEED[10];  //the speed of the motors in encoders/sec
float OLD_ERROR[10];   //the last error
task pidControl;
void slaveMotor(int slave, int master);
void driveStraight(int slave, int master, int masterPower, int slavePower);

void slaveMotor(int slave, int master) {         //make two motors go at the same power
  StopTask(pidControl);
	bool found = false;
    for(int i=0;(i<sizeof(MASTERS)/2)&&!found;i++) {
      if(MASTERS[i]==NULL) {                     //if the masters array has room add it
        MASTERS[i]=master;
        SLAVES[i]=slave;
        found =true;
        break;
      }
   }
   StartTask(pidControl);
}
void driveStraight(int slave, int master, int masterPower, int slavePower) {
	StopTask(pidControl);
  bool found = false;
   for(int i=0;i<sizeof(MONITORING)/2&&!found;i++) {
    if(MONITORING[i]==NULL||(MONITORING[i]!=NULL&&MONITORING[i]==master)) {                     //if the masters array has room add it
        MONITORING[i]=master;
        MONITORING[i+1]=slave;
        found=true;
    }
  } for(int i=0;i<sizeof(CONTROLLING)/2;i++) {    //add to the list of controlling and what it's tracking target is
            if(CONTROLLING[i]==NULL||(CONTROLLING[i]!=NULL&&CONTROLLING[i]==slave)) {
                CONTROLLING[i]=slave;
                MOTOR_TARGET[slave]=master;              //add following that motor
                break;
            }
        }
        motor[slave]=slavePower;
        motor[master]=masterPower;                               //turn on the motor
        for(int i=0;i<sizeof(MASTERS)/2;i++) {  //search for master motors
        	if(MASTERS[i]==slave||MASTERS[i]==master) {
        		if(MASTERS[i]==slave) {
        			motor[SLAVES[i]]=slavePower;
        			POWER[SLAVES[i]]=slavePower;
        		} else {
	        		motor[SLAVES[i]]=masterPower;
        			POWER[SLAVES[i]]=masterPower;
        		}
        	}
        }
        POWER[slave]=slavePower;
        POWER[master]=masterPower;  //store the powers
        StartTask(pidControl);
}
task pidControl {
    int target, i, compensate;
    float target_Value;
    while(1==1) {
        for(i=0;i<sizeof(MONITORING)/2;i++) {     //looks for the rate of motion for the motor
            if(MONITORING[i]!=NULL) {
              target=MONITORING[i];                      //the port number dealing with
              if(ERROR_P[target]!=NULL)
                OLD_ERROR[target]=ERROR_P[target];    //store the old error
              if(LAST_ENCODE[target]==NULL)
                LAST_ENCODE[target]=0;
              
              MOTOR_SPEED[target] = (nMotorEncoder[target] -LAST_ENCODE[target] )/(SAMPLE_TIME*1000); //finds the instant rate in encoders/sec
              if(bMotorReflected[target])   //if motor inverted invert the sign
              	MOTOR_SPEED[target]=-1*MOTOR_SPEED[target];
          }
        }
        for(i=0;i<sizeof(CONTROLLING)/2;i++){              //calculate the error and compensation
          if(CONTROLLING[i]!=NULL) {
            target=CONTROLLING[i];   //get the target
            target_Value = MOTOR_SPEED[MOTOR_TARGET[target]];    //get the target encoder
            ERROR_P[target] =  MOTOR_SPEED[target]-target_Value;
            ERROR_I[target] += ERROR_P[target]*(SAMPLE_TIME/1000); //get the integral add value times time splice
            if(OLD_ERROR[target]!=NULL)
              ERROR_D[target] = (ERROR_P[target]-OLD_ERROR[target])/(SAMPLE_TIME*1000);  //get the derivate aka slope
            else
              ERROR_D[target]=0;          //if no comparison just say no derivative
           compensate= (int) (PID_KP*ERROR_P[target]+PID_KI*ERROR_I[target]+PID_KD*ERROR_D[target]+0.5);  //get the compensating factor and round it
           POWER[target]+=compensate;  //add the compensating factor
          }
        }
        for(i=0;i<sizeof(SLAVES)/2;i++) {  //slave the motors
        	displayLCDNumber(0,0,i,1);
          if(SLAVES[i]!=NULL) {
            POWER[SLAVES[i]]=POWER[MASTERS[i]];  //take the power from the master
          }
        }
        for(i=0;i<sizeof(POWER)/2;i++) {
          if(POWER[i]!=NULL) {
            motor[i]=POWER[i];  //make the power so
          }
        }
        wait1Msec(SAMPLE_TIME);             //waits time before getting new info
    }
}